package 曲线多项式;

import 图表库.Jpolt;
import 图表库.JpoltCurve;

import java.awt.*;
import java.math.BigDecimal;
import java.util.Vector;

// 适用 2D平面的三次样条插值
public class Spline2D{
    public double[] S;
    public cubic_spline_interpolation Sx;
    public cubic_spline_interpolation Sy;
    public Vector<Double> rx = new Vector<>();
    public Vector<Double> ry = new Vector<>();
    public Vector<Double> ryaw = new Vector<>();
    public Vector<Double> rk = new Vector<>();
    public Vector<Double> s_data = new Vector<>();

    public Spline2D(double[] x, double[] y) {
        calc_s(x,y);
        this.Sx = new cubic_spline_interpolation(S,x);  // x 关于 S 的三次样条插值对象
        this.Sy = new cubic_spline_interpolation(S,y);  // y 关于 S 的三次样条插值对象
    }
    public void calc_s (double[] x, double[] y){  // 计算弧长
        double[] dx = new double[x.length-1];
        double[] dy = new double[y.length-1];
        double[] ds = new double[y.length-1];
        for (int i = 0; i < x.length-1; i++) {
            dx[i] = x[i+1] - x[i];
            dy[i] = y[i+1] - y[i];
            ds[i] = Math.sqrt( Math.pow(dx[i],2) + Math.pow(dy[i],2));
        }
        this.S = new double[ds.length+1];
        this.S[0] = 0.0;
        for (int i = 0; i < ds.length; i++) {
            S[i+1] = ds[i] + S[i];
        }
    }
    public double calc_positionX(double s){  // 利用三次样条插值将一个弧长值 S 转换为 对应的坐标 (x,y)
        return this.Sx.calc(s);
    }
    public double calc_positionY(double s){
        return this.Sy.calc(s);
    }
    public double calc_curvature(double s){  // 计算曲率
        double dx = this.Sx.calcd(s);
        double ddx = this.Sx.calcdd(s);
        double dy = this.Sy.calcd(s);
        double ddy = this.Sy.calcdd(s);
        return (ddy*dx-ddx*dy) / (Math.pow(dx,2)+Math.pow(dy,2));
    }
    public double calc_yaw(double s){  //  计算倾斜角
        double dx = this.Sx.calcd(s);
        double dy = this.Sy.calcd(s);
        return Math.atan2(dy,dx);
    }
    public void generate_target_course(){
        for (BigDecimal i = new BigDecimal(String.valueOf(0)); i.compareTo(new BigDecimal(String.valueOf(this.S[this.S.length-1]))) <= 0;) {
            s_data.add(i.doubleValue());
            i = i.add(new BigDecimal(String.valueOf(0.1)));
        }
        for (double i: s_data) {
            this.rx.add(this.calc_positionX(i));
            this.ry.add(this.calc_positionY(i));
            this.ryaw.add(this.calc_yaw(i));
            this.rk.add(this.calc_curvature(i));
        }
    }

}
class Main{
    public static void main(String[] args) {
        double[] x = {4,2,1};
        double[] y = {1,0,4};
        Spline2D n = new Spline2D(x,y);
        Vector<Double> s_data = new Vector<Double>();
        Vector<Double> rx = new Vector<Double>();
        Vector<Double> ry = new Vector<Double>();
        for (BigDecimal i = new BigDecimal(String.valueOf(0)); i.compareTo(new BigDecimal(String.valueOf(n.S[n.S.length-1]))) <= 0;) {
            s_data.add(i.doubleValue());
            i = i.add(new BigDecimal(String.valueOf(0.1)));
        }
        for (int i = 0;i<s_data.size();i++) {
            rx.add(n.calc_positionX(s_data.get(i)));
            ry.add(n.calc_positionY(s_data.get(i)));
        }
        JpoltCurve L1 = new JpoltCurve("L1", Color.orange);
        JpoltCurve L2 = new JpoltCurve("L2", Color.BLUE);
        for (int i = 0; i < rx.size(); i++) {
            L1.addPoint(rx.get(i), ry.get(i));
        }
        for (int i = 0; i < x.length; i++) {
            L2.addPoint(x[i],y[i]);
        }
        L2.VisibleLine = false;
        L2.Shape = JpoltCurve.rect;
        Jpolt jp = new Jpolt();
        jp.setRange(-0.6,5.3,1,-0.2,4.3,0.5);
        jp.addCurve(L2);
        jp.addCurve(L1);

    }
}
